package org.scalpel.mainApps;

import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStreamWriter;
import org.scalpel.mainApps.ScalpelActivity;


import org.scalpel.HHObjects.IOIOManager;
import org.scalpel.locodo.MotorInterface;
import org.scalpel.locodo.PID;

import android.content.Context;

import ioio.lib.api.DigitalInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.DigitalOutput.Spec.Mode;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.api.TwiMaster;


/**
 * 
 * @author Jerome Monceaux
 * Communication with encoder and motor using a ioio board.
 * Here we used 2 io pins from the ioio board to control the motor :
 * - 1 to deliver pwm signal
 * - 1 to control the rotation direction of the wheel.
 * 
 * The encoder is a ME210 : Mangetic Rotary encoder sells by acroname.
 * It is drive using the i2c interface.
 * 
 * We are using the 5V open drain mode with a pull up resistor for the output pwm.
 * But unfortunately, it give 5V by default at startup so the motor goes full speed.
 * To avoid this effect a 5V NOT gate is suppose to invert the pwm. The pwm duty cycle 
 * and the sign of the directional output suppose the presence of this gate.
 * 
 *///extends ScalpelActivity
public class IoioMotorControler  implements MotorInterface {

  private double fSpeed;            // current speed value
  private float fPidOuput;          // last output from pid (pid give acceleration, we need to integrate to send speed command to motor) 
  private int fEncVal;              // encoder value 
  private int fI2CDeviceAdress;     // i2c address of the encoder.
  private double fRealSpeedMean;    // Real motor speed
  private double fMaxRealSpeedinTickPerSecond; // Maximum real speed in tick per second
  
 private int fpin_MxIN1;
 private int fpin_MxIN2;
 private int findice;
 
  private PID fPid;                 // feedback control on the speed 
  
  final static byte[] REG_RESET = new byte[] {0x52};      // Reset register of the ME210
  final static byte[] REG_GETDISTANCE = new byte[] {0x44};// GetDistance Command Register
  
  // Encoder value response storage
  private byte[] fEncResponse = new byte[4+1];
  
  private IOIO fIoio;                // handle to ioio board interface
  private TwiMaster fTwi;            // handle to i2c interface
  
  private PwmOutput MxIN1;
  private PwmOutput MxIN2;
  
  static private int MEAN_NB = 20;   // Number of speed measurement used to cumpute the sliding mean 
  private int fIdMean = 0; 
  double[] fLastRealSpeed = new double[MEAN_NB];
  
  private static int NB_CALL_PER_SECOND = 1000 / IOIOManager.UPDATE_TIME;
  private double fMaxNbOfTickBySecond;
  
  static public int MES_MAX = 10000;   // Number of speed measurement max in the table
  public int Idmes=0;
  double[] fMesureSpeed = new double[MES_MAX];
  
  
  
  public IoioMotorControler(//int pwmPin, 
                           //int signPin,
                           int indice,
                           int pin_MxFB,
                           int pin_MxSF,
                           int pin_MxIN1,
                           int pin_MxIN2,
                            int i2CDeviceAdress, 
                            IOIO ioio, 
                            TwiMaster twi, 
                            double maxRealSpeedinTickPerSecond) throws ConnectionLostException {
//fPwmPinNumber = pwmPin;
//fSignPinNumber = signPin;
//   fpin_MxFB = pin_MxFB ;
//   fpin_MxSF = pin_MxSF;
   fpin_MxIN1 = pin_MxIN1;
   findice = indice;
   fpin_MxIN2 = pin_MxIN2;
    fSpeed = 0.f;
    fPidOuput = 0.f;
    fRealSpeedMean = 0.;
    fEncVal = 0;
    fIoio = ioio;
    fMaxRealSpeedinTickPerSecond = maxRealSpeedinTickPerSecond;
    fMaxNbOfTickBySecond = fMaxRealSpeedinTickPerSecond / NB_CALL_PER_SECOND;
    
    fPid = new PID();
    fPid.P = 1.;
    fPid.I = 0.;
    fPid.D = 0.;
    fPid.iLimit=10;
    
    for(int i = 0; i < MEAN_NB; i++)
    {
      fLastRealSpeed[i] = 0.;
    }
    
    fI2CDeviceAdress = i2CDeviceAdress;
    //Control MC33926---------------------------------------------------------------------------
    
     //Open pin IOIO
    
    //MxFB =   fIoio.openDigitalInput(fpin_MxFB);
    //MxSF = fIoio.openDigitalInput(fpin_MxSF);
    MxIN1 =  fIoio.openPwmOutput(new DigitalOutput.Spec(fpin_MxIN1, Mode.NORMAL), 20000);
    MxIN2 =  fIoio.openPwmOutput(new DigitalOutput.Spec(fpin_MxIN2, Mode.NORMAL), 20000);

    /**
    fPwmOutput = fIoio.openPwmOutput(new DigitalOutput.Spec(fPwmPinNumber, Mode.OPEN_DRAIN), 30000);
    fSignOutput = fIoio.openDigitalOutput(new DigitalOutput.Spec(fSignPinNumber, Mode.OPEN_DRAIN), false);
   */
    fTwi = twi;
    
    //---------------------------------------------------------------------------------------------------
    
    //set motor speed to 0
    //fPwmOutput.setDutyCycle(1.f); // suppose the NOT gate
   // MxD2.setDutyCycle(1.f);
    MxIN1.setDutyCycle(0.f);
    MxIN2.setDutyCycle(0.f);
    
    //Reset Encoder
    try {
      byte[] emptyResp = new byte[] {0}; // workaround
      fTwi.writeRead(fI2CDeviceAdress, false, REG_RESET, REG_RESET.length, emptyResp, 0);
    }catch (InterruptedException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
  }
  
  public void setPid(double P, double I, double D)
  {
    fPid.P = P;
    fPid.I = I;
    fPid.D = D;
  }
  
  public void close()
  {
    //if( fPwmOutput != null )
      //fPwmOutput.close();
	  
	    if( MxIN1 != null )
	    	MxIN1.close();
	  
	    if( MxIN2 != null )
	    	MxIN2.close();
  } 
  
  @Override
  synchronized public int getEncoderValue() {
    return fEncVal;
  }

  @Override
  public void setSpeed(double speed) {
    fSpeed = speed;
  }

  @Override
  public void update(){
    double sError = fSpeed;
    
    sError = fSpeed - fRealSpeedMean;
    fPid.setCurrValue(sError, 1);
    fPidOuput = (float)fPid.update();
    
    float output = fPidOuput; 
    try {
      fTwi.writeReadAsync(fI2CDeviceAdress, false, REG_GETDISTANCE, REG_GETDISTANCE.length, fEncResponse, fEncResponse.length);
      
      /**
      if( output > 0) // uses inversion bit to configure the H bridge 
        fSignOutput.write(false);
      else{
        fSignOutput.write(true);
        output = -output;
      }*/
      
      /* if test motor separate*/
      //if (findice == 2)
      //{
    	  if( output < 0)
          {
    		  output = -output;
            MxIN1.setDutyCycle(0.f);
            MxIN2.setDutyCycle( output);
          }
        else{
        	
          MxIN2.setDutyCycle(0.f);
          MxIN1.setDutyCycle( output);
          //; 
        }
     // }
      //Pour les test.
      //if (findice == 3)
      //{
          /**{if( output < 0)
          {        output = -output;
          MxIN2.setDutyCycle(0.f);
          MxIN1.setDutyCycle( output); 

          }
        else{        	 
            MxIN1.setDutyCycle(0.f);
            MxIN2.setDutyCycle( output);
        }*/
      //}
      
     // }
     // fPwmOutput.setDutyCycle(1.f - output); // suppose 5V NOT gate

      
    } catch (ConnectionLostException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
    
    //res.waitReady(); // do not wait for acknowledge, otherwise cycle time will explode
    int lastEncVal = fEncVal;
    synchronized(this){
      fEncVal = byteArrayToInt(fEncResponse);
    }

    // Compute the speed mean
    fLastRealSpeed[fIdMean] = (double)(fEncVal - lastEncVal) / fMaxNbOfTickBySecond;
    fIdMean++;
    if( fIdMean >= MEAN_NB) fIdMean = 0;
    
    double sum = 0;
    for(int i=0; i < MEAN_NB; i++){
      sum += fLastRealSpeed[i];
    }
    fRealSpeedMean = sum / (double)MEAN_NB;
  }
  
  private static int byteArrayToInt(byte[] b){ // byte rearrangement
    int value = ((b[4] & 0xff) << 24) | ((b[3] & 0xff) << 16) | ((b[2] & 0xff) << 8) | (b[1] & 0xff);
    return value;
  }
}
